#ifndef ALGORITHMCONTROL_HPP
#define	ALGORITHMCONTROL_HPP

#include "ros/ros.h"

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_eigen.h>

#include "geometry_msgs/Twist.h"
#include "visualization.hpp"
#include "epuckDriver/SetOdometry.h"
#include "epuckDriver/GetOdometry.h"

#include "camControl/CameraControlMsg.h"

class AlgorithmControl {
public:

    AlgorithmControl();

    virtual void control();

    virtual void setOdometry(int numRobot,int vel_v, int vel_w) {
    }

    virtual void updateRobotsPosition() {

    }

    void camControlCallBack(camControl::CameraControlMsg msg);    

    static void kron(gsl_matrix *K, gsl_matrix *A, gsl_matrix *B);
    static int alter_dim(int n);
    static void alter_set(gsl_matrix *A, int n);
    static void kron_matrix_with_eye(gsl_matrix * K, gsl_matrix * A, int sizeEye);
    static void kron_eye_with_matrix(gsl_matrix * K, gsl_matrix * A, int sizeEye);

protected:

    INFO_ROBOT robots[NUM_MAX_ROBOTS];
    
    std::string epucksSetOdom[NUM_MAX_ROBOTS];
    std::string epucksGetOdom[NUM_MAX_ROBOTS];
    std::string epucksSetVel[NUM_MAX_ROBOTS];
    
    epuckDriver::SetOdometry request[NUM_MAX_ROBOTS];
    epuckDriver::GetOdometry response[NUM_MAX_ROBOTS];    
        
    ros::NodeHandle nControl;
    ros::Publisher pub[NUM_MAX_ROBOTS];    
    ros::ServiceClient cliGetOdom[NUM_MAX_ROBOTS];
    ros::ServiceClient cliSetOdom[NUM_MAX_ROBOTS];

    geometry_msgs::Twist twist;
    geometry_msgs::Pose2D pose2d[NUM_MAX_ROBOTS];

    gsl_matrix *Adj;
    gsl_matrix *A;
    gsl_matrix *B;
    gsl_matrix *hp;
    gsl_matrix *temp;
    gsl_matrix *h;
    gsl_matrix *D;
    gsl_matrix *L;
    gsl_matrix *Leig;
    gsl_matrix *F;
    gsl_matrix *Ae;
    gsl_matrix *Be;
    gsl_matrix *Fe;
    gsl_matrix *Le;
    gsl_matrix *Temp1;
    gsl_matrix *Temp2;
    gsl_matrix *Form1;
    gsl_matrix *y;
    gsl_matrix *dy;
    gsl_matrix *pos;
    gsl_matrix *posLocalization;

    gsl_vector *yv;
    gsl_vector *tempv;
    gsl_vector *tempv1;
    gsl_vector *tempv2;
    gsl_vector *tempvM;
    gsl_vector *tempvM1;
    gsl_vector *posi;
    gsl_vector *hv;
    gsl_vector *vetCot; // = gsl_vector_calloc(6);
    gsl_vector *y0;

    gsl_vector_complex *eigL1;
    gsl_vector_complex *eigForm1;

    gsl_eigen_nonsymm_workspace *w;

    double k;
    double lonQuad;
    double sample; //0.20 + 0.24;		// Sample time for 6 e-pucks
    double tFinalMax;
    double tFinal; // t_final em (ms)

    int numIter, loopControl;
    int dimAdj, numMaxRobots;
    int il;

    bool allDetected;
};

#endif	/* ALGORITHMCONTROL_HPP */

